#include <eigen_conversions/eigen_msg.h>
#include <ros/ros.h>
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <tf2_ros/transform_listener.h>

#include "ground_segmentation/ground_segmentation.h"
#include "pcl/filters/filter.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/crop_box.h>
#include <Depth/DrivableSeg.h>
#include "glog/logging.h"
#include "tools/file_manager.hpp"
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Dense>
#include <nav_msgs/GridCells.h>
#include <iostream>
#include <sstream>

using namespace Eigen;
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_cloud,noground_cloud,slope_cloud,fullPointCloud;
pcl::PointCloud<pcl::PointXYZI>::Ptr cluster_cloud,filtered_slope_cloud;

bool zvision_reverse = false;

//string newpath = "/home/fjy/Desktop/catkin_ws/chucao/linefit_ground_segmentation/src/linefit_ground/PCDFILE/ece-grass2/2.pcd";
//string newpath2 = "/home/fjy/Desktop/catkin_ws/chucao/linefit_ground_segmentation/src/linefit_ground/PCDFILE/ece-grass2/2.pcd.txt";

int TP = 0;
int FP = 0;
int FN = 0;
int pcdlabel_num = 0;
class SegmentationNode {
    enum clusterpurpose{
        obstacle = 0,
        plane = 1,
    };

    struct CenterLength{
        vector<Eigen::Vector3f> Center;
        vector<Eigen::Vector3f> minLength;
        vector<Eigen::Vector3f> maxLength;


    }CL;
    ros::NodeHandle nh_;
    ros::Publisher ground_pub_;
    ros::Publisher obstacle_pub_;
    ros::Publisher slope_cloud_pub_;
    ros::Publisher noground_cloud_pub_;
    ros::Publisher pub_MarkerArray_;
    ros::Publisher GridCell_pub_;

    tf2_ros::Buffer tf_buffer_;
    tf2_ros::TransformListener tf_listener_{tf_buffer_};
    GroundSegmentationParams params_;
    pcl::CropBox<pcl::PointXYZ> boundary_box_filter_;
    pcl::CropBox<pcl::PointXYZ> nearby_box_filter_;

    std::shared_ptr<DrivableSeg> dsMethod;
//    std::shared_ptr<GroundSegmentation> segmenter;

    visualization_msgs::MarkerArray marker_array;
    visualization_msgs::Marker bbox_marker;

    std_msgs::Header cloudHeader;
    double CAR_HEIGHT;
public:
    SegmentationNode(ros::NodeHandle &nh,
                     const std::string &ground_topic,
                     const std::string &obstacle_topic,
                     const GroundSegmentationParams &params,
                     const bool &latch = false) : params_(params),nh_(nh) {
        ground_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>(ground_topic, 1, latch);
        obstacle_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>(obstacle_topic, 1, latch);
        noground_cloud_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/noground_cloud", 1, latch);
        slope_cloud_pub_ = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/slope_cloud", 1, latch);
        pub_MarkerArray_ = nh.advertise<visualization_msgs::MarkerArray>("/Mark_array",1);
        GridCell_pub_ = nh.advertise<nav_msgs::GridCells>("/gridCell", 1);

        nh.param("CAR_HEIGHT", CAR_HEIGHT,0.408);
        boundary_box_filter_.setMin(Eigen::Vector4f(-20, -20, -CAR_HEIGHT-0.2, 1.0));
        boundary_box_filter_.setMax(Eigen::Vector4f(20, 20, 2, 1.0));
        boundary_box_filter_.setNegative(false);//保留框内的

        //滤去没有接收到的点，没有接收到的点都是0，0，0，
        nearby_box_filter_.setMin(Eigen::Vector4f(-0.5, -0.5, -0.5, 1.0));
        nearby_box_filter_.setMax(Eigen::Vector4f(0.5, 0.5, 0.5, 1.0));
        nearby_box_filter_.setNegative(true);//保留框外的

        dsMethod = std::make_shared<DrivableSeg>(nh);
//        gpfmethod = std::make_shared<GroundPlaneFit>();

//        segmenter = std::make_shared<GroundSegmentation>(nh_,params_);

        bbox_marker.header.frame_id = "zvision_lidar";
//        bbox_marker.header.stamp = ros::Time::now();
        bbox_marker.ns = "";
        bbox_marker.color.r = 0.0f;
        bbox_marker.color.g = 1.0f;
        bbox_marker.color.b = 0.0f;
        bbox_marker.color.a = 0.5;
//    bbox_marker.pose.orientation
        bbox_marker.lifetime = ros::Duration();
        bbox_marker.frame_locked = true;
        bbox_marker.type = visualization_msgs::Marker::CUBE;
        bbox_marker.action = visualization_msgs::Marker::ADD;
    }

    void scanCallback(const sensor_msgs::PointCloud2ConstPtr laserCloudMsg) {


        std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();
        // todo 测试一下每次进来都要重新定义类和改为指针的形式时间效率

        copyPointCloud(laserCloudMsg);

        pcl::PointCloud<pcl::PointXYZ> cloud_transformed;

        std::vector<int> labels;
        //计算平均时间消耗
        static int EnterNumber = 1;
        static double fp_total_ms = 0;
        static double fp_avg_ms = 0;
        ground_cloud->clear();
        noground_cloud->clear();
        cluster_cloud->clear();
        slope_cloud->clear();
        filtered_slope_cloud->clear();
//
        GroundSegmentation segmenter(nh_,params_);
        segmenter.segment(*fullPointCloud, &labels);
        for (size_t i = 0; i < fullPointCloud->size(); ++i) {
            if (labels[i] != 0) {
                ground_cloud->push_back(fullPointCloud->points[i]);
                if(labels[i] == 2)
                    slope_cloud->push_back(fullPointCloud->points[i]);
            }
            else noground_cloud->push_back(fullPointCloud->points[i]);
        }
//
//
        bool is_stair_flag = false;//判断斜坡是不是楼梯
        bool temp_flag = false;//这个不起作用，只是为了满足输入要求
        dsMethod->ComputeDrivableArea(slope_cloud,*filtered_slope_cloud,clusterpurpose::plane,500,is_stair_flag);

        dsMethod->ComputeDrivableArea(noground_cloud,*cluster_cloud,clusterpurpose::obstacle,30,temp_flag);

        if(is_stair_flag){
            *cluster_cloud += *filtered_slope_cloud;//如果是楼梯那就把整个点云并入障碍物当中
            filtered_slope_cloud->clear();
        }

//        //计算耗时
//        std::chrono::high_resolution_clock::time_point end = std::chrono::high_resolution_clock::now();
//        std::chrono::duration<double, std::milli> fp_ms = end - start;
//        fp_total_ms += fp_ms.count();
//        fp_avg_ms = fp_total_ms/EnterNumber;
//        EnterNumber++;
//        std::cout << "AvgCluster! Took " << fp_avg_ms << "ms\n";
//        std::cout << "TotalCluster! Took " << fp_total_ms << "ms\n";
//
        PublishCloud();
        PublishBBox(cluster_cloud);
//        PublishGridCells(cluster_cloud);
        ComputeIoU();
    }

    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {

        cloudHeader = laserCloudMsg->header;
//        pcl::fromROSMsg(*laserCloudMsg, *fullPointCloud);
        static int pcdnum = 1;
        string str_pcdnum;
        str_pcdnum = std::to_string(pcdnum);
        string newpath = "/home/fjy/Desktop/catkin_ws/chucao/PCDFILE2_labed/PCD/" + str_pcdnum + ".pcd";
        pcdnum++;
        fullPointCloud->clear();
        if (pcl::io::loadPCDFile<pcl::PointXYZ> (newpath, *fullPointCloud) == -1) //* load the file
        {
            PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
            return ;
        }


        // Remove Nan points
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*fullPointCloud, *fullPointCloud, indices);

        int Cloudsize = fullPointCloud->points.size();

        //zvision 如果雷达是上下颠倒的
        if(zvision_reverse) {
            for (size_t i = 0; i < Cloudsize; i++) {
                fullPointCloud->points[i].x = -fullPointCloud->points[i].x;
                fullPointCloud->points[i].z = -fullPointCloud->points[i].z;
            }
        }

        //只要20*20以内的点云
        std::vector<int> boxIndices_boundary;
        boundary_box_filter_.setInputCloud(fullPointCloud);
        boundary_box_filter_.filter(boxIndices_boundary);//
        boundary_box_filter_.filter(*fullPointCloud);

        std::vector<int> boxIndices_boundary2;
        nearby_box_filter_.setInputCloud(fullPointCloud);
        nearby_box_filter_.filter(boxIndices_boundary2);//
        nearby_box_filter_.filter(*fullPointCloud);
    }

    void PublishCloud(){
        sensor_msgs::PointCloud2 laserCloudTemp;

        if (ground_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*ground_cloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            ground_pub_.publish(laserCloudTemp);
        }
        if (obstacle_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*cluster_cloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            obstacle_pub_.publish(laserCloudTemp);
        }
        if (noground_cloud_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*noground_cloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            noground_cloud_pub_.publish(laserCloudTemp);
        }

        if (slope_cloud_pub_.getNumSubscribers() != 0) {
            pcl::toROSMsg(*filtered_slope_cloud, laserCloudTemp);
            laserCloudTemp.header.stamp = cloudHeader.stamp;
            laserCloudTemp.header.frame_id = "zvision_lidar";
            slope_cloud_pub_.publish(laserCloudTemp);
        }
    }

    void PublishBBox(const pcl::PointCloud<pcl::PointXYZI>::Ptr clustered_cloud){

////普通的计算方法
        if(clustered_cloud->empty())
            return;

        visualization_msgs::MarkerArray marker_array;
        pcl::PointCloud<pcl::PointXYZ> TempCloud;
        pcl::PointXYZ SumPoint,maxpoint,minpoint,Center,Length,temppoint;
        maxpoint.x = -100;maxpoint.y = -100;maxpoint.z = -100;
        minpoint.x = 100;minpoint.y = 100;minpoint.z = 100;
        size_t CntNum = 0;
        size_t marker_id = 0;
        pcl::PointXYZ min;	//xyz的最小值
        pcl::PointXYZ max;	//xyz的最大值
        CL.Center.clear();CL.minLength.clear();CL.maxLength.clear();

        static size_t max_marker_size_ = 0;
        int flag_intensity = clustered_cloud->begin()->intensity;
        int SIzeCloud = clustered_cloud->size();
        size_t cntcloudsize = 0;
        Vector3f Center_Length;
        for(auto iter = clustered_cloud->begin();iter != clustered_cloud->end();iter++){
            if(iter->intensity == flag_intensity){
                SumPoint.x += iter->x;
                SumPoint.y += iter->y;
                SumPoint.z += iter->z;

                temppoint.x = iter->x;
                temppoint.y = iter->y;
                temppoint.z = iter->z;
                TempCloud.push_back(temppoint);

                CntNum++;
                cntcloudsize++;
                continue;
            }
            pcl::getMinMax3D(TempCloud,min,max);	//在聚类的坐标系中计算最大最小值

            bbox_marker.id = marker_id;
            bbox_marker.color.a = 0.5;
            bbox_marker.pose.position.x = SumPoint.x/CntNum;
            bbox_marker.pose.position.y = SumPoint.y/CntNum;
            bbox_marker.pose.position.z = SumPoint.z/CntNum;
            bbox_marker.scale.x = max.x - min.x;
            bbox_marker.scale.y = max.y - min.y;
            bbox_marker.scale.z = max.z - min.z;
            bbox_marker.header.stamp = ros::Time::now();
            marker_array.markers.push_back(bbox_marker);

            Center_Length.x() = bbox_marker.pose.position.x;
            Center_Length.y() = bbox_marker.pose.position.y;
            Center_Length.z() = bbox_marker.pose.position.z;
            CL.Center.push_back(Center_Length);
            Center_Length.x() = bbox_marker.pose.position.x - bbox_marker.scale.x/2;
            Center_Length.y() = bbox_marker.pose.position.y - bbox_marker.scale.y/2;
            Center_Length.z() = bbox_marker.pose.position.z - bbox_marker.scale.z/2;
            CL.minLength.push_back(Center_Length);
            Center_Length.x() = bbox_marker.pose.position.x + bbox_marker.scale.x/2;
            Center_Length.y() = bbox_marker.pose.position.y + bbox_marker.scale.y/2;
            Center_Length.z() = bbox_marker.pose.position.z + bbox_marker.scale.z/2;
            CL.maxLength.push_back(Center_Length);

            flag_intensity = iter->intensity;
            iter--;
            marker_id++;
            CntNum = 0;
            maxpoint.x = -100;maxpoint.y = -100;maxpoint.z = -100;
            minpoint.x = 100;minpoint.y = 100;minpoint.z = 100;
            SumPoint.x = 0;SumPoint.y = 0;SumPoint.z = 0;
            TempCloud.clear();
        }

        pcl::getMinMax3D(TempCloud,min,max);	//在聚类的坐标系中计算最大最小值
        bbox_marker.id = marker_id;
        bbox_marker.color.a = 0.5;
        bbox_marker.pose.position.x = SumPoint.x/CntNum;
        bbox_marker.pose.position.y = SumPoint.y/CntNum;
        bbox_marker.pose.position.z = SumPoint.z/CntNum;
        bbox_marker.scale.x = max.x - min.x;
        bbox_marker.scale.y = max.y - min.y;
        bbox_marker.scale.z = max.z - min.z;
        bbox_marker.header.stamp = ros::Time::now();
        marker_array.markers.push_back(bbox_marker);

        Center_Length.x() = bbox_marker.pose.position.x;
        Center_Length.y() = bbox_marker.pose.position.y;
        Center_Length.z() = bbox_marker.pose.position.z;
        CL.Center.push_back(Center_Length);
        Center_Length.x() = bbox_marker.pose.position.x - bbox_marker.scale.x/2;
        Center_Length.y() = bbox_marker.pose.position.y - bbox_marker.scale.y/2;
        Center_Length.z() = bbox_marker.pose.position.z - bbox_marker.scale.z/2;
        CL.minLength.push_back(Center_Length);
        Center_Length.x() = bbox_marker.pose.position.x + bbox_marker.scale.x/2;
        Center_Length.y() = bbox_marker.pose.position.y + bbox_marker.scale.y/2;
        Center_Length.z() = bbox_marker.pose.position.z + bbox_marker.scale.z/2;
        CL.maxLength.push_back(Center_Length);

        static int labeled_txt = 1;
        string str_pcdtxtnum = std::to_string(labeled_txt);
        string newpath2 = "/home/fjy/Desktop/catkin_ws/chucao/PCDFILE2_labed/Label/" + str_pcdtxtnum + ".pcd.txt";
        labeled_txt++;

        fstream  f(newpath2);//创建一个fstream文件流对象
        string  line; //保存读入的每一行
        string name;
        pcdlabel_num = 0;
        while(getline(f,line))//会自动把\n换行符去掉
        {
            marker_id++;
            istringstream ss(line);
            ss>>name;
            bbox_marker.id = marker_id;
            bbox_marker.color.a = 0.5;
            bbox_marker.color.r = 0.0f;
            bbox_marker.color.g = 0.0f;
            bbox_marker.color.b = 1.0f;
            ss >> bbox_marker.pose.position.x;
            ss >> bbox_marker.pose.position.y;
            ss >> bbox_marker.pose.position.z;
            ss >> bbox_marker.scale.x;
            ss >> bbox_marker.scale.y;
            ss >> bbox_marker.scale.z;
            bbox_marker.header.stamp = ros::Time::now();
            marker_array.markers.push_back(bbox_marker);
            pcdlabel_num++;
        }

        bbox_marker.color.r = 0.0f;
        bbox_marker.color.g = 1.0f;
        bbox_marker.color.b = 0.0f;
        if (marker_array.markers.size() > max_marker_size_){
            max_marker_size_ = marker_array.markers.size();

        }
        else{
            for (size_t i = marker_id+1; i < max_marker_size_; ++i)
            {
                bbox_marker.id = i;
                bbox_marker.color.a = 0;
                bbox_marker.pose.position.x = 0;
                bbox_marker.pose.position.y = 0;
                bbox_marker.pose.position.z = 0;
                bbox_marker.scale.x = 0;
                bbox_marker.scale.y = 0;
                bbox_marker.scale.z = 0;
                marker_array.markers.push_back(bbox_marker);
                ++marker_id;
            }
        }

        pub_MarkerArray_.publish(marker_array);

//// 增加Marker的旋转
/*    if(clustered_cloud->empty())
        return;

    visualization_msgs::MarkerArray marker_array;
    pcl::PointCloud<pcl::PointXYZ> TempCloud;
    pcl::PointXYZ SumPoint,maxpoint,minpoint,temppoint;
    maxpoint.x = -100;maxpoint.y = -100;maxpoint.z = -100;
    minpoint.x = 100;minpoint.y = 100;minpoint.z = 100;
    size_t marker_id = 0;
    Eigen::Matrix3f normal_,cov;
    Eigen::Vector4f pc_mean;
    Eigen::Vector3f pretranslate_;
    pcl::PointXYZ min;	//xyz的最小值
    pcl::PointXYZ max;	//xyz的最大值

    static size_t max_marker_size_ = 0;
    int flag_intensity = clustered_cloud->begin()->intensity;
    for(auto iter = clustered_cloud->begin();iter != clustered_cloud->end();iter++){
        if(iter->intensity == flag_intensity){

            temppoint.x = iter->x;
            temppoint.y = iter->y;
            temppoint.z = iter->z;
            TempCloud.push_back(temppoint);

            continue;
        }
        bbox_marker.id = marker_id;
        bbox_marker.color.a = 0.5;
        pcl::computeMeanAndCovarianceMatrix(TempCloud, cov, pc_mean);
        JacobiSVD<MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
        normal_ = (svd.matrixU());
        normal_.col(2) = normal_.col(0).cross(normal_.col(1));//计算第三个方向为前两个方向的叉乘，不然之后显示的框会出错
        Quaternionf q = Quaternionf(normal_);//不用转置，这里表示三个方向，跟旋转啥的没有关系
        q.normalize();
        Isometry3f T = Isometry3f::Identity();//设置旋转矩阵，把世界坐标系中的点云变换到聚类的坐标系中
        T.rotate(normal_.transpose());
        pretranslate_ = -normal_.transpose()*pc_mean.head(3);
        T.pretranslate(pretranslate_);
        pcl::transformPointCloud(TempCloud,TempCloud,T);
        pcl::getMinMax3D(TempCloud,min,max);	//在聚类的坐标系中计算最大最小值

        bbox_marker.pose.orientation.x = q.x();
        bbox_marker.pose.orientation.y = q.y();
        bbox_marker.pose.orientation.z = q.z();
        bbox_marker.pose.orientation.w = q.w();
        bbox_marker.pose.position.x = 0.5*(max.x + min.x) + pc_mean.x();//pc_mean平移量，再加上在聚类坐标系中的值
        bbox_marker.pose.position.y = 0.5*(max.y + min.y) + pc_mean.y();
        bbox_marker.pose.position.z = 0.5*(max.z + min.z) + pc_mean.z();
        bbox_marker.scale.x = max.x - min.x;
        bbox_marker.scale.y = max.y - min.y;
        bbox_marker.scale.z = max.z - min.z;
        bbox_marker.header.stamp = ros::Time::now();
        marker_array.markers.push_back(bbox_marker);

        flag_intensity = iter->intensity;
        iter--;
        marker_id++;
        maxpoint.x = -100;maxpoint.y = -100;maxpoint.z = -100;
        minpoint.x = 100;minpoint.y = 100;minpoint.z = 100;
        SumPoint.x = 0;SumPoint.y = 0;SumPoint.z = 0;
        TempCloud.clear();
    }

    bbox_marker.id = marker_id;
    bbox_marker.color.a = 0.5;
    pcl::computeMeanAndCovarianceMatrix(TempCloud, cov, pc_mean);
    JacobiSVD<MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
    normal_ = (svd.matrixU());

    normal_.col(2) = normal_.col(0).cross(normal_.col(1));
    Quaternionf q = Quaternionf(normal_);//不用转置，这里表示三个方向
    q.normalize();
    Isometry3f T = Isometry3f::Identity();
    T.rotate(normal_.transpose());
    pretranslate_ = -normal_.transpose()*pc_mean.head(3);

    T.pretranslate(pretranslate_);
    pcl::transformPointCloud(TempCloud,TempCloud,T);

    pcl::getMinMax3D(TempCloud,min,max);	//获取所有点中的坐标最值

    bbox_marker.pose.orientation.x = q.x();
    bbox_marker.pose.orientation.y = q.y();
    bbox_marker.pose.orientation.z = q.z();
    bbox_marker.pose.orientation.w = q.w();
    bbox_marker.pose.position.x = 0.5*(max.x + min.x) + pc_mean.x();//
    bbox_marker.pose.position.y = 0.5*(max.y + min.y) + pc_mean.y();
    bbox_marker.pose.position.z = 0.5*(max.z + min.z) + pc_mean.z();
    bbox_marker.scale.x = max.x - min.x;
    bbox_marker.scale.y = max.y - min.y;
    bbox_marker.scale.z = max.z - min.z;
    bbox_marker.header.stamp = ros::Time::now();
    marker_array.markers.push_back(bbox_marker);

    if (marker_array.markers.size() > max_marker_size_){
        max_marker_size_ = marker_array.markers.size();
    }
    else{
        for (size_t i = marker_id+1; i < max_marker_size_; ++i)
        {
            bbox_marker.id = i;
            bbox_marker.color.a = 0;
            bbox_marker.pose.position.x = 0;
            bbox_marker.pose.position.y = 0;
            bbox_marker.pose.position.z = 0;
            bbox_marker.scale.x = 0;
            bbox_marker.scale.y = 0;
            bbox_marker.scale.z = 0;
            marker_array.markers.push_back(bbox_marker);
            ++marker_id;
        }
    }

    pub_MarkerArray_.publish(marker_array);*/
    }

    void PublishGridCells(const pcl::PointCloud<pcl::PointXYZI>::Ptr clustered_cloud){
        float resolution = 0.05;//一格长宽5cm
        int range = 10;
        int Width = 10*ceil(1.0/resolution);
        int Length = 2*10*ceil(1.0/resolution);
        int inv_resolution = ceil(1.0/resolution);
        nav_msgs::GridCells cells;

        cv::Mat GridMap = cv::Mat::zeros(Width,Length,CV_8S);

        size_t CloudSize = clustered_cloud->size();
        cells.header.frame_id = "zvision_lidar";
        cells.cell_height= 0.05;
        cells.cell_width = 0.05;


        geometry_msgs::Point obstacle;
        int x,y;
        obstacle.z = -0.408;
        for(size_t i = 0;i<CloudSize;i++){
            x = floor((clustered_cloud->points[i].x + 10)*inv_resolution);
            if(x >= Length || x < 0)
                continue;
            y = floor((clustered_cloud->points[i].y)*inv_resolution);
            if(y >= Length || y < 0)
                continue;
            if(GridMap.at<int8_t>(y,x) == 1)
                continue;

            GridMap.at<int8_t>(y,x) = 1;
            obstacle.x = floor((clustered_cloud->points[i].x)*inv_resolution)*resolution;
            obstacle.y = floor((clustered_cloud->points[i].y)*inv_resolution)*resolution;

            cells.cells.push_back(obstacle);
        }
        GridCell_pub_.publish(cells);
    }

    void ComputeIoU(){

        Eigen::Vector3f center,minaxis,maxaxis,length;
        static int labeled_txt = 1;
        string str_pcdtxtnum = std::to_string(labeled_txt);
        if(labeled_txt ==8)
            int cc = 0;
        string newpath2 = "/home/fjy/Desktop/catkin_ws/chucao/PCDFILE2_labed/Label/" + str_pcdtxtnum + ".pcd.txt";
        labeled_txt++;

        fstream  f(newpath2);//创建一个fstream文件流对象
        string  line; //保存读入的每一行
        size_t boxsize = CL.Center.size();
        static float SumIoU = 0;
        static int total_obj_num = 0;
        float axisXmin,axisXmax,axisYmin,axisYmax,axisZmin,axisZmax;
        float intersection_area,union_area;
        vector<int> FN_count(pcdlabel_num);
        vector<int> FP_count(CL.maxLength.size());
        vector<int> TP_count(pcdlabel_num);
        std::fill(FN_count.begin(), FN_count.end(),1);
        std::fill(FP_count.begin(), FP_count.end(),1);
        std::fill(TP_count.begin(), TP_count.end(),1);

        int real_obstacle_count=0;
        string name;
        for(int j = 0;j<pcdlabel_num;j++)
        {
            getline(f,line);
            bool FN_flag = true;
            bool TP_flag = false;
            istringstream ss(line);
            ss>>name;
            ss>>center.x()>>center.y()>>center.z()>>length.x()>>length.y()>>length.z();
            minaxis.x() = center.x()-length.x()/2;
            minaxis.y() = center.y()-length.y()/2;
            minaxis.z() = center.z()-length.z()/2;
            maxaxis.x() = center.x()+length.x()/2;
            maxaxis.y() = center.y()+length.y()/2;
            maxaxis.z() = center.z()+length.z()/2;

            for(size_t i = 0 ; i<boxsize;i++){
                maxaxis.x() < CL.maxLength[i].x() ? (axisXmax = maxaxis.x()):(axisXmax = CL.maxLength[i].x());
                minaxis.x() > CL.minLength[i].x() ? (axisXmin = minaxis.x()):(axisXmin = CL.minLength[i].x());
                if(axisXmax-axisXmin < 0)continue;
                maxaxis.y() < CL.maxLength[i].y() ? (axisYmax = maxaxis.y()):(axisYmax = CL.maxLength[i].y());
                minaxis.y() > CL.minLength[i].y() ? (axisYmin = minaxis.y()):(axisYmin = CL.minLength[i].y());
                if(axisYmax-axisYmin < 0)continue;
                maxaxis.z() < CL.maxLength[i].z() ? (axisZmax = maxaxis.z()):(axisZmax = CL.maxLength[i].z());
                minaxis.z() > CL.minLength[i].z() ? (axisZmin = minaxis.z()):(axisZmin = CL.minLength[i].z());
                if(axisZmax-axisZmin < 0)continue;

//                if(TP_flag == false){
//                    TP++;
//                }
                TP++;
                FP_count.at(i)=0;//表示我的框有用把障碍物检测出来了。不是FP
                FN_flag = false;//表示这个障碍物被正确的检测到
//                TP_count[real_obstacle_count]=1;//表示这个障碍物被正确的检测出来了
                intersection_area = (axisXmax-axisXmin)*(axisYmax-axisYmin)*(axisZmax-axisZmin);
                union_area = (CL.maxLength[i].x()-CL.minLength[i].x())*(CL.maxLength[i].y()-CL.minLength[i].y())*(CL.maxLength[i].z()-CL.minLength[i].z())
                        + (maxaxis.x() - minaxis.x())*(maxaxis.y() - minaxis.y())*(maxaxis.z() - minaxis.z())
                        -intersection_area;
                SumIoU += intersection_area/union_area;
//                std::cout<<"mIoU:"<<SumIoU/TP<<endl;

//                real_obstacle_count++;
//                break;
            }

            if(FN_flag == true){//如果有障碍但是我没检测到
                FN++;
            }


        }
        for(int k = 0;k<CL.maxLength.size();k++){
            if(FP_count.at(k) == 1)
                FP++;
        }

        total_obj_num += pcdlabel_num;
        std::cout<<"---------"<<endl;
        std::cout<<"pcdnum:"<<labeled_txt<<endl;
        std::cout<<"mIoU:"<<SumIoU/TP<<endl;
        std::cout<<"ACC"<<(float)TP/(TP+FN+FP)<<endl;
        std::cout<<"precision"<<(float)TP/(TP+FP)<<endl;
        std::cout<<"recall:"<<(float)TP/(TP+FN)<<endl;
        std::cout<<"total_obj_num:"<<total_obj_num<<endl;
    }
};


    int main(int argc, char **argv) {
        ros::init(argc, argv, "ground_segmentation");

        ros::NodeHandle nh("~");

        // Do parameter stuff.
        GroundSegmentationParams params;
        nh.param("visualize", params.visualize, params.visualize);
        nh.param("n_bins", params.n_bins, params.n_bins);
        nh.param("n_segments", params.n_segments, params.n_segments);
        nh.param("max_dist_to_line", params.max_dist_to_line, params.max_dist_to_line);
        nh.param("max_slope", params.max_slope, params.max_slope);
        nh.param("min_slope", params.min_slope, params.min_slope);
        nh.param("long_threshold", params.long_threshold, params.long_threshold);
        nh.param("max_long_height", params.max_long_height, params.max_long_height);
        nh.param("max_start_height", params.max_start_height, params.max_start_height);
        nh.param("sensor_height", params.sensor_height, params.sensor_height);
        nh.param("line_search_angle", params.line_search_angle, params.line_search_angle);
        nh.param("n_threads", params.n_threads, params.n_threads);
        nh.param("fitting_line_count", params.fitting_line_count, params.fitting_line_count);
        nh.param("MinDegreeSlope", params.MinDegreeSlope, params.MinDegreeSlope);
        nh.param("zvision_reverse", zvision_reverse,zvision_reverse);

        // Params that need to be squared.
        double r_min, r_max, max_fit_error;
        if (nh.getParam("r_min", r_min)) {
            params.r_min_square = r_min * r_min;
        }
        if (nh.getParam("r_max", r_max)) {
            params.r_max_square = r_max * r_max;
        }
        if (nh.getParam("max_fit_error", max_fit_error)) {
            params.max_error_square = max_fit_error * max_fit_error;
        }

        std::string ground_topic, obstacle_topic, input_topic,WORK_SPACE_PATH;
        bool latch;
        nh.param<std::string>("input_topic", input_topic, "/zvision_lidar_points");
        nh.param<std::string>("ground_output_topic", ground_topic, "/ground_cloud");
        nh.param<std::string>("obstacle_output_topic", obstacle_topic, "/obstacle_cloud");
        nh.param("latch", latch, false);
        nh.param<std::string>("WORK_SPACE_PATH", WORK_SPACE_PATH, "/home/fjy/Desktop/catkin_ws/chucao/linefit_ground_segmentation/src/linefit_ground");

        fullPointCloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        ground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        noground_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        slope_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>());
        filtered_slope_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());
        cluster_cloud.reset(new pcl::PointCloud<pcl::PointXYZI>());

        google::InitGoogleLogging(argv[0]);
        FLAGS_log_dir = WORK_SPACE_PATH + "/Log";
        FLAGS_alsologtostderr = true;
        FLAGS_colorlogtostderr = true;
        FLAGS_log_prefix = true;
        FLAGS_logbufsecs = 0;
        FileManager::CreateDirectory(FLAGS_log_dir);

        // Start node.
        SegmentationNode node(nh, ground_topic, obstacle_topic, params, latch);
        ros::Subscriber cloud_sub;
        cloud_sub = nh.subscribe(input_topic, 1, &SegmentationNode::scanCallback, &node);
        ros::spin();
    }
